﻿/*!
 * @file ImageBackgroundSubtractorMixtureOfGaussianAdaptiveNum.cpp
 * @author 橋本敦史
 * @date Last Change:2010/Jun/18.
 */
#include "ImageBackgroundSubtractorDynamicMixtureOfGaussianAdaptiveNum.h"

using namespace std;

using namespace mmpl;
using namespace mmpl::image;



/*!
 * @brief デフォルトコンストラクタ
 */
BackgroundSubtractorDynamicMixtureOfGaussianAdaptiveNum::BackgroundSubtractorDynamicMixtureOfGaussianAdaptiveNum(double threshold,double alpha,double HP, double deadline,double sigma, double mergeThreshold):BackgroundSubtractorDynamicMixtureOfGaussian(threshold,alpha,sigma),HP(HP),deadline(deadline),mergeThreshold(mergeThreshold){
}

/*!
 * @brief デストラクタ
 */
BackgroundSubtractorDynamicMixtureOfGaussianAdaptiveNum::~BackgroundSubtractorDynamicMixtureOfGaussianAdaptiveNum(){

}

/*!
 * @brief 分布数を減らす方向での更新を行う(増やす部分はupdateByForeGround..内)
 * @param rhoVec 分布の統合において重み付き和を取る際の重み
 * */
void BackgroundSubtractorDynamicMixtureOfGaussianAdaptiveNum::updateEraseDistributions(MixtureOfSphericalGaussian* mog){
	for(unsigned int i=0;i<mog->weights.size();i++){
		if(mog->weights[i]<deadline){
			//! 余分な分布を削除する
			mog->weights.erase(mog->weights.begin()+i);
			mog->distributions.erase(mog->distributions.begin()+i);
			i--;
		}
	}

	for(unsigned int i=0;i<mog->weights.size();i++){
		//! 似ている分布があれば、それらを統合する
		for(unsigned int j=i+1;j<mog->weights.size();j++){
			if(		mergeThreshold > abs(mog->distributions[i].means[0] - mog->distributions[j].means[0]) &&
					mergeThreshold > abs(mog->distributions[i].means[1] - mog->distributions[j].means[1]) &&
					mergeThreshold > abs(mog->distributions[i].means[2] - mog->distributions[j].means[2]) ){
				mog->weights[i] += mog->weights[j];
				mog->distributions[i].sigma = weightedSum(mog->distributions[i].sigma, mog->distributions[j].sigma, mog->weights[i], mog->weights[j]);
				for(int c=0;c<3;c++){
					mog->distributions[i].means[c] = weightedSum(mog->distributions[i].means[c], mog->distributions[j].means[c], mog->weights[i], mog->weights[j]);
				}
				mog->weights.erase(mog->weights.begin()+j);
				mog->distributions.erase(mog->distributions.begin()+j);
			}
		}
	}
	mog->weights.regularize();
}
/*!
 * @brief 分布とのマッチング結果と前景と判定された色を利用して背景モデルを更新する
 * @param mog 更新する混合ガウス分布:w
 * 
 * @param col 更新する画像の前景と判定された画素値
 * @param probs 現状の混合ガウス分布とのマッチング結果
 */
void BackgroundSubtractorDynamicMixtureOfGaussianAdaptiveNum::updateByForeGroundColor(MixtureOfSphericalGaussian* mog, const Color& col,const MmplVector& probs){
	int minMachingScoreIdx = 0;
	double minMachingScore = DBL_MAX;
	MmplVector matchingScore = mog->weights;
	int c;

	for(unsigned int i=0;i<mog->weights.size();i++){
		matchingScore[i] /= mog->distributions[i].sigma;
		if(minMachingScore > matchingScore[i]){
			minMachingScore = matchingScore[i];
			minMachingScoreIdx = i;
		}
	}


	//! 最も照合度(matchingScore)が低い分布の分散値を借りて新しい分布を追加する
	SphericalGaussian newDist;
	newDist.means.resize(3,0.0);
	for(c=0;c<3;c++){
		newDist.means[c] = col[c];
	}
	newDist.sigma = mog->distributions[minMachingScoreIdx].sigma;
	mog->distributions.push_back(newDist);
	mog->weights.push_back(HP);
	mog->weights.regularize();
}

/*!
 * @brief 分布とのマッチング結果と背景と判定された色を利用して背景モデルを更新する
 * @param mog 更新する混合ガウス分布
 * @param col 更新する画像の背景と判定された画素値
 * @param probs 現状の混合ガウス分布とのマッチング結果
 */
void BackgroundSubtractorDynamicMixtureOfGaussianAdaptiveNum::updateByBackGroundColor(MixtureOfSphericalGaussian* mog, const Color& col,const MmplVector& probs){
	double rho, r_rho; // 学習率、alphaとprob[i]から計算したものをこれに格納
	double dsigma; // 分散の変化量を一時的に格納
	int c;

	unsigned int MaxDist = probs.argmax();
	for(unsigned int i=0;i<mog->weights.size();i++){
		mog->weights[i] *= 1-alpha;
		if(i == MaxDist && probs[i] > threshold){
			//! マッチした分布についてはalphaを足す
			mog->weights[i] += alpha;
			//! マッチした分布については平均と分散を更新
			rho = alpha * probs[i];
			r_rho = 1.0-rho;
			dsigma = 0;
			for(c=0;c<3;c++){
				mog->distributions[i].means[c] *= r_rho;
				mog->distributions[i].means[c] += rho*col[c];
				dsigma += pow(mog->distributions[i].means[c]-col[c],2);
			}
			mog->distributions[i].sigma *= r_rho;
			mog->distributions[i].sigma += rho*dsigma;
		}
	}
	mog->weights.regularize();

	//! 分布数を減らす方向の更新
	updateEraseDistributions(mog);

}

double BackgroundSubtractorDynamicMixtureOfGaussianAdaptiveNum::weightedSum(double val1,double val2, double weight1,double weight2){
	double value;
	value = weight1 * val1 + weight2 * val2;
	value /= weight1 + weight2;
	return value;

}
